#include "StdAfx.h"

#include "OpticalFlow.h"
#include "imageIterator.h"
#include <highgui.h>
// #include <fstream>
// 
// using namespace std;

// #define CV_IMAGE_ELEM( image, elemtype, row, col )       \
// 	(((elemtype*)((image)->imageData + (image)->widthStep*(row)))[(col)])

OpticalFlow::OpticalFlow(void):output(NULL), NextImg(NULL), PreImg(NULL), CurImg(NULL),EigImg(NULL), TmpImg(NULL),
				PyramidCur(NULL), PyramidPrev(NULL), m_fEnergyfw(0.0f), m_fEnergybw(0.0f), m_fEntropyFw(0.0f), m_fEntropyBw(0.0f), 
				m_fEntropyJoint(0.0f),bg_model(NULL), pImgBgModel(NULL), pFrameMat(NULL), pFrMat(NULL), pBkMat(NULL),
				m_fMutualInfo(0.0f), m_fNMutualInfo(0.0f), m_igridwidth(0), m_igridheight(0), m_iFeatureno(0), m_pfndFeatures(NULL), 
				m_pfndFeatures2(NULL), m_pOpticalflow(NULL), m_pOpticalflow2(NULL), m_pFeatureGrid(NULL), 
				m_pFeatureGridtracked(NULL), m_pFeatureGridtracked2(NULL), 	PyramidCur2(NULL), PyramidPrev2(NULL),
				m_pOptFeatError(NULL), m_pWriter(NULL), m_iMItypePrev(-1), m_iMItypeCur(-1), m_fMutualInfoPrev(0.0), m_fNMutualInfoPrev(0.0)
#ifdef TEST							
				,FrModel(NULL), BkModel(NULL)
#endif
/*, InputMat(NULL), FrMat(NULL), BkMat(NULL)*/
{
	//EigImg = cvCreateImage
	OptWnd = cvSize(3,3);
	OptTerCri = cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3);
	FeatureNo = MAXFeat;

	cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5, 0, 1);
	cvInitFont(&m_Font, CV_FONT_HERSHEY_SIMPLEX, 0.9, 1.0, 0, 2, CV_AA);

	Feat1storage = cvCreateMemStorage(0);
	Feat2storage = cvCreateMemStorage(0);

	FeatVec1 = cvCreateSeq(0, sizeof(CvSeq), sizeof(FeatureOP), Feat1storage);
	FeatVec2 = cvCreateSeq(0, sizeof(CvSeq), sizeof(FeatureOP), Feat2storage);

	FWHist = new float[HistBin];
	memset(FWHist, 0, HistBin*sizeof(float));
	BWHist = new float[HistBin];
	memset(BWHist, 0, HistBin*sizeof(float));

	FVJointHist = new float*[HistBin];
	for (int i = 0; i < HistBin; i++)
	{
		FVJointHist[i] = new float[HistBin];
		memset(FVJointHist[i], 0, HistBin*sizeof(float));
	}
	//static int count = 0;
	fenergyFile.open("E:\\Desktop\\opencv_video\\OutputFiles\\energy.m", ios::out);
	fenergydiffFile.open("E:\\Desktop\\opencv_video\\OutputFiles\\energydiff.m", ios::out);
	fmutualinfoFile.open("E:\\Desktop\\opencv_video\\OutputFiles\\mutual.m", ios::out);
	fnmutualinfoFile.open("E:\\Desktop\\opencv_video\\OutputFiles\\nmutual.m", ios::out);
	fdynamicFile.open("E:\\Desktop\\opencv_video\\OutputFiles\\dynamic.m", ios::out);
	fforratio.open("E:\\Desktop\\opencv_video\\OutputFiles\\forratio.m", ios::out);

	fenergyFile<<"fenergy = ["<<endl;
	fenergydiffFile<<"fenergydiff = ["<<endl;
	fmutualinfoFile<<"fmutualinfo = ["<<endl;
	fnmutualinfoFile<<"fnmutualinfo = ["<<endl;
	fdynamicFile<<"fdynamic=["<<endl;
	fforratio<<"fforratio=["<<endl;


#ifdef TEST
	//cvNamedWindow("foreground",1);
	//cvNamedWindow("background",1);
#endif

}

bool OpticalFlow::isInitialized(IplImage *image)
{
	return output && (output->width == image->width) 
		&& (output->height == image->height);
}

void OpticalFlow::initialize(IplImage *image)
{
	cvReleaseImage(&output);
	cvReleaseImage(&NextImg);
	cvReleaseImage(&PreImg);
	cvReleaseImage(&CurImg);
	cvReleaseImage(&EigImg);
	cvReleaseImage(&TmpImg);
	cvReleaseImage(&PyramidCur);
	cvReleaseImage(&PyramidPrev);
#ifdef TEST
	cvReleaseImage(&FrModel);
	cvReleaseImage(&BkModel);
#endif
	output= cvCreateImage(cvSize(image->width,image->height),
		image->depth, image->nChannels);
	//output->origin= image->origin;

	//NextImg = cvCreateImage(cvSize(image->width,image->height), image->depth, image->nChannels);
	PreImg = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_8U, 1);
	NextImg = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_8U, 1);
	CurImg = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_8U, 1);
#ifdef TEST

	FrModel = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_8U, 1);
	BkModel = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_8U, 1);

#endif
	EigImg = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_32F, 1);
	TmpImg = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_32F, 1);
	PyramidCur = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_32F, 1);
	PyramidPrev = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_32F, 1);
	PyramidCur2 = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_32F, 1);
	PyramidPrev2 = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_32F, 1);


	m_igridwidth = image->width/Blockwidth - 1;
	m_igridheight = image->height/Blockheight - 1;

	m_iFeatureno = m_igridheight*m_igridwidth;

	m_pFeatureGrid = new CvPoint2D32f[m_iFeatureno];
	m_pFeatureGridtracked = new CvPoint2D32f[m_iFeatureno];
	m_pFeatureGridtracked2 = new CvPoint2D32f[m_iFeatureno];

	m_pOpticalflow = new CvPoint2D32f[m_iFeatureno];
	m_pOpticalflow2 = new CvPoint2D32f[m_iFeatureno];

	m_pOptFeatError = new float[m_iFeatureno];

	for (int i = 0; i < m_igridheight; i++)
	{
		for (int j =0; j < m_igridwidth; j++)
		{
			m_pFeatureGrid[i*m_igridwidth + j].x = Blockwidth*(j+1);
			m_pFeatureGrid[i*m_igridwidth + j].y = Blockheight*(i+1);
			m_pFeatureGridtracked[i*m_igridwidth + j].x = Blockwidth*(j+1);
			m_pFeatureGridtracked[i*m_igridwidth + j].y = Blockheight*(i+1);
			m_pFeatureGridtracked2[i*m_igridwidth +j].x = Blockwidth*(j+1);
			m_pFeatureGridtracked2[i*m_igridwidth +j].y = Blockheight*(i+1);
		}
	}
	//m_iFeatureno = m_igridheight*m_igridwidth;
	m_pfndFeatures = new char[m_iFeatureno];
	m_pfndFeatures2 = new char[m_iFeatureno];

//	CvSize cvImgSize = cvSize(image->width, image->height);

// 	if (m_pWriter == NULL)
// 	{
// 		m_pWriter = cvCreateVideoWriter("E:\\Desktop\\opencv_video\\abnormal.avi", CV_FOURCC('X', 'V', 'I', 'D'), 25, cvImgSize, 1);
// 	}
}


void OpticalFlow::process(IplImage *image, int framenum)
{
	FeatureOP FeatopFw;
	FeatureOP FeatopBw;

	float fFWSum = 0.0f;
	float fBWSum = 0.0f;
	float fJointSum = 0.0f;

	float fEnergyfwtemp = 0.0f;
	float fEnergybwtemp = 0.0f;
	float fEnergyDifftemp = 0.0f;

	// 	CvMat* pFrameMat = NULL;
	// 	CvMat* pFrMat = NULL;
	// 	CvMat* pBkMat = NULL;


	if (framenum == 0)
	{
		cvConvertImage(image, NextImg, CV_CVTIMG_FLIP);
		cvConvertImage(image, PreImg, CV_CVTIMG_FLIP);
		cvConvertImage(image, CurImg, CV_CVTIMG_FLIP);
		cvConvertImage(image, output, CV_CVTIMG_FLIP);

		//background modeling
		pBkMat = cvCreateMat(image->height, image->width, CV_32FC1);
		pFrMat = cvCreateMat(image->height, image->width, CV_32FC1);
		pFrameMat = cvCreateMat(image->height, image->width, CV_32FC1);

		cvConvert(CurImg, pFrameMat);
		cvConvert(CurImg, pFrMat);
		cvConvert(CurImg, pBkMat);

		//gaussian model
		//bg_model = (CvGaussBGModel*)cvCreateGaussianBGModel(image, 0);
	}
	else if (framenum == 1)
	{
		cvConvertImage(image, CurImg, CV_CVTIMG_FLIP);
		cvConvertImage(image, NextImg, CV_CVTIMG_FLIP);
		cvConvertImage(image, output, CV_CVTIMG_FLIP);

		cvConvert(CurImg, pFrameMat);

		cvAbsDiff(pFrameMat, pBkMat, pFrMat);
#ifdef TEST
		cvThreshold(pFrMat, FrModel, 20, 255.0, CV_THRESH_BINARY);
#endif
		cvRunningAvg(pFrameMat, pBkMat, 0.2/*0.003*/, 0);

#ifdef TEST
		cvConvert(pBkMat, BkModel);
		//cvShowImage("foreground", FrModel);
		//cvShowImage("background", BkModel);
#endif
		//gaussian model
		//cvUpdateBGStatModel(image, (CvBGStatModel *)bg_model );
	}
	else
	{
		cvConvertImage(image, NextImg, CV_CVTIMG_FLIP);
		//cvConvertImage(image, BkImg, CV_CVTIMG_FLIP);
		cvConvertImage(image, output, CV_CVTIMG_FLIP);

		cvConvert(CurImg, pFrameMat);

		cvAbsDiff(pFrameMat, pBkMat, pFrMat);

#ifdef TEST
		cvThreshold(pFrMat, FrModel, 20, 255.0, CV_THRESH_BINARY);
#endif

		cvRunningAvg(pFrameMat, pBkMat, 0.2/*0.003*/, 0);

#ifdef TEST
		cvConvert(pBkMat, BkModel);
		//cvShowImage("foreground", FrModel);
		//cvShowImage("background", BkModel);
#endif
		//gaussian model
		//cvUpdateBGStatModel(image, (CvBGStatModel *)bg_model );

		cvGoodFeaturesToTrack(/*PreImg*/CurImg, EigImg, TmpImg, Features, &FeatureNo, .01, 5.0, 0, 3, 0, 0.4);
		cvFindCornerSubPix(CurImg, Features, FeatureNo, cvSize(3, 3), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3));

		memset(PyramidPrev->imageData, 0, sizeof(float)*(image->width+8)*(image->height/3));
		memset(PyramidCur->imageData, 0, sizeof(float)*(image->width+8)*(image->height/3));
		cvCalcOpticalFlowPyrLK(CurImg, NextImg, PyramidCur, PyramidPrev, Features, Featurestrackedfw, FeatureNo, OptWnd, 5, OptfndFeaturesfw, \
			OptFeaError, OptTerCri, 0);

		memset(PyramidPrev->imageData, 0, sizeof(float)*(image->width+8)*(image->height/3));
		memset(PyramidCur->imageData, 0, sizeof(float)*(image->width+8)*(image->height/3));
		cvCalcOpticalFlowPyrLK(CurImg, PreImg, PyramidCur, PyramidPrev, Features, Featurestrackedbw, FeatureNo, OptWnd, 5, OptfndFeaturesbw, \
			OptFeaError, OptTerCri, 0);

		cvCopy(CurImg, PreImg, NULL);
		cvCopy(NextImg, CurImg, NULL);

		//draw feature vectors
		//drawvectors(Features, Featurestrackedfw, OptfndFeaturesfw, FeatureNo, CV_RGB(0, 255, 0), &m_fEnergyfw);
		//drawvectors(Features, Featurestrackedbw, OptfndFeaturesbw, FeatureNo, CV_RGB(255, 0, 0), &m_fEnergybw);

		//Draw Vectors

		CvPoint2D32f p, q, v;
		double dangle, dhypotenuse;

		// 		fFWSum = 0.0f;
		// 		fBWSum = 0.0f;
		// 		fJointSum = 0.0;
		resethist(FWHist, BWHist, FVJointHist);

		for (int i = 0; i < FeatureNo; i++)
		{
			//forward
			p = Features[i];
			unsigned char temp = 1;
#ifdef TEST
			temp = CV_IMAGE_ELEM( FrModel, unsigned char, cvRound(p.y), cvRound(p.x));
#endif

			if (/*temp*//*CV_IMAGE_ELEM( FrModel, unsigned char, cvRound(p.y), cvRound(p.x))*/1)
			{
				//q = Featurestrackedfw[i];
				q.x = float(int(Featurestrackedfw[i].x+0.5f));
				q.y = float(int(Featurestrackedfw[i].y+0.5f));

				v = cvPoint2D32f(q.x-p.x, q.y-p.y);
				dangle = atan2((p.y - q.y), (p.x - q.x));
				dhypotenuse = sqrt((p.y - q.y)*(p.y - q.y) + (p.x - q.x)*(p.x - q.x));

				FeatopFw.Feature = p;
				FeatopFw.Vector = v;
				FeatopFw.angle = dangle;
				FeatopFw.hypotenuse = dhypotenuse;

				if (dhypotenuse > EPSILON && dhypotenuse < 10.0)
				{
					if (CV_IMAGE_ELEM( FrModel, unsigned char, cvRound(p.y), cvRound(p.x)))
					{
						fEnergyfwtemp+=float(dhypotenuse*dhypotenuse);
						drawvector(p, dangle, dhypotenuse, OptfndFeaturesfw[i], CV_RGB(0, 255, 0));
					}

					//cvSeqPush(FeatVec1, &FeatopFw);
					// 				FWHist[int((dangle+2*PI)/(2*PI/HistBin))%HistBin] += (float)dhypotenuse;
					// 				FWSum += (float)dhypotenuse;

					FWHist[int((dangle+2*PI)/(2*PI/HistBin))%HistBin] += (float)1.0f;
					fFWSum += (float)1.0f;

				}

				//backward
				//q = Featurestrackedbw[i];
				q.x = float(int(Featurestrackedbw[i].x+0.5f));
				q.y = float(int(Featurestrackedbw[i].y+0.5f));
				v = cvPoint2D32f(p.x-q.x, p.y-q.y);
				dangle = atan2(q.y-p.y, q.x-p.x);
				dhypotenuse = sqrt((p.y - q.y)*(p.y - q.y) + (p.x - q.x)*(p.x - q.x));


				FeatopBw.Feature = p;
				FeatopBw.Vector = v;
				FeatopBw.angle = dangle;
				FeatopBw.hypotenuse = dhypotenuse;

				if (dhypotenuse > EPSILON && dhypotenuse < 10.0)
				{
					if (CV_IMAGE_ELEM( FrModel, unsigned char, cvRound(p.y), cvRound(p.x)))
					{
						fEnergybwtemp +=float(dhypotenuse*dhypotenuse);
						drawvector(p, dangle, dhypotenuse, OptfndFeaturesbw[i], CV_RGB(255, 0, 0));
					}
					//cvSeqPush(FeatVec2, &FeatopBw);		
					// 				BWHist[int((dangle+2*PI)/(2*PI/HistBin))%HistBin] +=(float)dhypotenuse;
					// 				BWSum += (float)dhypotenuse;

					BWHist[int((dangle+2*PI)/(2*PI/HistBin))%HistBin] +=1.0f;
					fBWSum += 1.0f;
				}

				//joint histgram
				if (FeatopFw.hypotenuse > EPSILON && FeatopBw.hypotenuse > EPSILON && FeatopFw.hypotenuse < 10.0 && FeatopBw.hypotenuse < 10.0)
				{
					FVJointHist[int((FeatopFw.angle+2*PI)/(2*PI/HistBin))%HistBin][int((FeatopBw.angle+2*PI)/(2*PI/HistBin))%HistBin] +=(float)1.0f;
					fJointSum += 1.0f;
				}
			}
		}
		float fforground = 0.0f;
		for (int i=0; i< image->height; i++)
		{
			for (int j = 0; j<image->width; j++)
			{
				if (CV_IMAGE_ELEM( FrModel, unsigned char, i, j))//cvRound(p.y), cvRound(p.x)))
				{
					fforground += 1.0f;
				}
			}
		}
		float fbratio = fforground/(image->width*image->height);

		fEnergyDifftemp = (fEnergyfwtemp >fEnergybwtemp) ? (fEnergyfwtemp - fEnergybwtemp) : (fEnergybwtemp - fEnergyfwtemp);

		m_fEnergyfw = fEnergyfwtemp;
		m_fEnergybw = fEnergybwtemp;
		m_fEnergyDiff = fEnergyDifftemp;

		fforratio<<fbratio<<endl;
		fenergyFile<<m_fEnergyfw*fbratio<<endl;
		fenergydiffFile <<m_fEnergyDiff<<endl;
		// 		fenergyFile.flush();
		// 		fenergydiffFile.flush();

		//normalize histogram
		fstream file1, file2, file3;
		file1.open("E:\\Desktop\\opencv_video\\OutputFiles\\FwHist.txt", ios::out);
		file2.open("E:\\Desktop\\opencv_video\\OutputFiles\\BwHist.txt", ios::out);
		file3.open("E:\\Desktop\\opencv_video\\OutputFiles\\JointHist.txt", ios::out);

		for (int i = 0; i < HistBin; i++)
		{
			if (fFWSum > EPSILON)
			{
				//file1<<i<<" "<<FWHist[i]<<endl;
				file1<<FWHist[i]<<endl;
				FWHist[i] /= fFWSum;
			}
			if (fBWSum > EPSILON)
			{
				//file2<<i<<" "<<BWHist[i]<<endl;
				file2<<BWHist[i]<<endl;
				BWHist[i] /= fBWSum;
			}
			for (int j = 0; j < HistBin; j++)
			{			
				if (fJointSum > EPSILON)
				{
					file3<<FVJointHist[i][j]<<" ";
					FVJointHist[i][j] /= fJointSum;
				}
			}
			file3<<endl;
		}
		//file1.flush();
		file1.close();
		file2.close();
		file3.close();
		//test

		//joint histogram
		// 		for (int i = 0; i < HistBin; i++)
		// 		{
		// 			for (int j = 0; j < HistBin; j++)
		// 			{
		// 				FVJointHist[i][j] = FWHist[i]*BWHist[j];
		// 			}
		// 		}		

		float sum1=0.0f, sum2=0.0f, sum3 = 0.0f;
		for(int i = 0; i < HistBin; i++)
		{
			sum1+=FWHist[i];
			sum2+=BWHist[i];
			for (int j = 0; j < HistBin; j++)
			{
				sum3+=FVJointHist[i][j];
			}
		}

		//Mutual Information
		if (mutualinfo(FWHist, BWHist, FVJointHist))
		{
			//draw Mutual Infromation
			// 			CvScalar misc = cvGet2D(output, 20, output->height-40);
			// 			CvPoint mipoint = cvPoint(20, output->height-40);
			// 			char mitext[50];
			m_fMutualInfo = m_fEntropyFw+m_fEntropyBw-m_fEntropyJoint;
			// 			sprintf(mitext, "Mutual Info.: #%6.3f", m_fMutualInfo);

			//			drawtext(output, mitext, misc, mipoint);

			//Normalize, use the metric D'(x,y) = 1 - I(X,Y)/max(H(X), H(Y));
			float fmaxH = m_fEntropyFw >m_fEntropyBw ? m_fEntropyFw: m_fEntropyBw;
			//m_fMutualInfo = m_fEntropyFw+m_fEntropyBw-m_fEntropyJoint;
			//m_fNMutualInfo = 1 - m_fMutualInfo/fmaxH;
			if (fmaxH>EPSILON)
			{
				m_fNMutualInfo = 1 - m_fMutualInfo/fmaxH;
			}
			else
				m_fNMutualInfo = 0.0f;
			// 
			if (m_fNMutualInfo > 1.0f - EPSILON)
			{
				m_fNMutualInfo = 1.0f;
			}
			else if (m_fNMutualInfo < EPSILON)
			{
				m_fNMutualInfo = 0.0f;
			}
			// 			CvScalar nmisc = cvGet2D(output, 20, output->height-40);
			// 			CvPoint nmipoint = cvPoint(20, output->height-60);
			//			char nmitext[50];
			//			sprintf(nmitext, "Normalized Mutual Info.: #%6.3f",m_fNMutualInfo/*(1-(m_fEntropyFw+EntropyBw-m_fEntropyJoint)/(m_fEntropyFw>=EntropyBw?m_fEntropyFw: EntropyBw)) */);

			//			drawtext(output, nmitext, nmisc, nmipoint);
			fmutualinfoFile<<m_fMutualInfo<<endl;
			fnmutualinfoFile<<m_fNMutualInfo<<endl;
		}

		// 		if (m_fNMutualInfo > 1.0f - EPSILON)
		// 		{
		// 			m_fNMutualInfo = 1.0f;
		// 		}
		// 		else if (m_fNMutualInfo < EPSILON)
		// 		{
		// 			m_fNMutualInfo = 0.0f;
		// 		}

		//fdynamicFile<<m_fEnergyDiff*m_fNMutualInfo<<endl;
		fdynamicFile<<fabs(m_fNMutualInfo-m_fNMutualInfoPrev)<<endl;
		m_fNMutualInfoPrev = m_fNMutualInfo;
		//draw energy difference
		//		CvScalar sc = cvGet2D(output,20,output->height-20);
		//		CvPoint point = cvPoint(20, output->height-20);
		//		char text[50];
		//		sprintf(text,"Energy Difference: #%6.3f",m_fEnergyfw-m_fEnergybw);

		//		drawtext(output, text, sc, point);

		memset(Features, 0, sizeof(CvPoint2D32f)*MAXFeat);
		memset(Featurestrackedfw, 0, sizeof(CvPoint2D32f )*MAXFeat);
		memset(Featurestrackedbw, 0, sizeof(CvPoint2D32f )*MAXFeat);

		memset(OptfndFeaturesfw, 0, sizeof(char)*MAXFeat);
		memset(OptfndFeaturesbw, 0, sizeof(char)*MAXFeat);

		memset(FWHist, 0, HistBin*sizeof(float));
		memset(BWHist, 0, HistBin*sizeof(float));
	}
	if(cvWriteFrame(m_pWriter, image) == 0)
	{
		AfxMessageBox(_T("Can not write to files"));
		return;
	}
}

void OpticalFlow::drawvectors( CvPoint2D32f *Feats, CvPoint2D32f *Featstrked,char *OptfndFeats, int FeatNo, CvScalar Linecolor, float *Energy )
{
	//draw feature vectors
	//float Energy = 0.0;
	*Energy = 0.0f;
	for (int i = 0; i < FeatNo; i++)
	{
		if (OptfndFeats[i] == 0) 
		{
			continue;
		}
		float alpha = 0.4f;
		int iLineThick = 1;
		//CvScalar cvLineCol = CV_RGB(0, 255, 0);
		CvPoint p, q;
		p.x = (int)Feats[i].x;
		p.y = (int)Feats[i].y;
		q.x = (int)Featstrked[i].x;
		q.y = (int)Featstrked[i].y;

		double angle = atan2(double(p.y - q.y), double(p.x - q.x));
		double hypotenuse = sqrt(double(/*square*/(p.y - q.y)*(p.y - q.y) + /*square*/(p.x - q.x)*(p.x - q.x)));
		*Energy += (float)(hypotenuse*hypotenuse);

		if (hypotenuse>1.5)
		{
			q.x = (int)(p.x - 6*hypotenuse*cos(angle));
			q.y = (int)(p.y - 6*hypotenuse*sin(angle));

			cvLine(output, p, q, /*cvLineCol*/Linecolor, iLineThick, CV_AA, 0);

			p.x = (int)(q.x + 6*alpha*hypotenuse*cos(angle + PI/8));
			p.y = (int)(q.y + 6*alpha*hypotenuse*sin(angle + PI/8));

			cvLine(output, p, q, /*cvLineCol*/Linecolor, iLineThick, CV_AA, 0);

			p.x = (int)(q.x + 6*alpha*hypotenuse*cos(angle - PI/8));
			p.y = (int)(q.y + 6*alpha*hypotenuse*sin(angle - PI/8));

			cvLine(output, p, q, /*cvLineCol*/Linecolor, iLineThick, CV_AA, 0);
		}
	}
	// 	CvScalar sc;
	// 	char text[50];
	// 	sprintf(text,"Energy: #%6f",Energy);
	// 
	// 	sc= cvGet2D(output,20,20);
	// 	if (sc.val[1]>128) 
	// 		cvPutText(output, text, cvPoint(20,output->height-20), &(font), cvScalar(0,0,0));
	// 	else
	// 		cvPutText(output, text, cvPoint(20,output->height-20), &(font), cvScalar(255,255,255));

}

void OpticalFlow::release()
{
	cvDestroyAllWindows();
	cvReleaseImage(&output);
	cvReleaseImage(&NextImg);
	cvReleaseImage(&PreImg);
	cvReleaseImage(&CurImg);
	cvReleaseImage(&EigImg);
	cvReleaseImage(&TmpImg);
	cvReleaseImage(&PyramidCur);
	cvReleaseImage(&PyramidPrev);
	cvReleaseImage(&PyramidCur2);
	cvReleaseImage(&PyramidPrev2);

#ifdef TEST
	cvReleaseImage(&FrModel);
	cvReleaseImage(&BkModel);
#endif

	cvReleaseMemStorage(&Feat1storage);
	cvReleaseMemStorage(&Feat2storage);

	//gaussian model
	//cvReleaseBGStatModel((CvBGStatModel**)&bg_model);

	cvReleaseMat(&pFrameMat);
	cvReleaseMat(&pFrMat);
	cvReleaseMat(&pBkMat);

	fenergyFile<<"];"<<"hold on;"<<"plot(fenergy)"<<endl;
	fenergydiffFile<<"];"<<"hold on;"<<"plot(fenergydiff)"<<endl;
	fmutualinfoFile<<"];"<<"hold on;"<<"plot(fmutualinfo)"<<endl;
	fnmutualinfoFile<<"];"<<"hold on;"<<"plot(fnmutualinfo)"<<endl;
	fdynamicFile<<"];"<<"plot(fdynamic)"<<endl;
	fforratio<<"];"<<"plot(fforratio)"<<endl;

	fenergyFile.close();
	fenergydiffFile.close();
	fmutualinfoFile.close();
	fnmutualinfoFile.close();
	fdynamicFile.close();
	fforratio.close();

	delete [] FWHist;
	FWHist = NULL;
	delete [] BWHist;
	BWHist = NULL;
	for (int i = 0; i < HistBin; i++)
	{
		delete [] FVJointHist[i];
	}
	delete [] FVJointHist; 
	FVJointHist = NULL;

	// 	for (int i= 0; i < m_igridheight; i++)
	// 	{
	// 		delete [] m_pFeatureGrid[i];
	// 		delete [] m_pFeatureGridtracked[i];
	// 		delete [] m_pFeatureGridtracked2[i];
	// 	}
	if (m_pFeatureGrid)
	{
		delete [] m_pFeatureGrid;
		m_pFeatureGrid = NULL;
	}
	if (m_pFeatureGridtracked)
	{
		delete [] m_pFeatureGridtracked;
		m_pFeatureGridtracked = NULL;
	}
	if (m_pFeatureGridtracked2)
	{
		delete  [] m_pFeatureGridtracked2;
		m_pFeatureGridtracked2 = NULL;
	}

	if (m_pfndFeatures)
	{
		delete [] m_pfndFeatures;
		m_pfndFeatures = NULL;
	}
	if (m_pfndFeatures2)
	{	
		delete [] m_pfndFeatures2;
		m_pfndFeatures2 = NULL;
	}
	if (m_pOpticalflow)
	{
		delete  [] m_pOpticalflow;
		m_pOpticalflow = NULL;
	}

	if (m_pOpticalflow2)
	{
		delete [] m_pOpticalflow2;
		m_pOpticalflow2 = NULL;
	}

	if (m_pOptFeatError)
	{
		delete [] m_pOptFeatError;
		m_pOptFeatError = NULL;
	}

#ifdef TEST
	cvDestroyWindow("background");
	cvDestroyWindow("foreground");
#endif

	cvReleaseVideoWriter(&m_pWriter);
}

void OpticalFlow::drawvector( CvPoint2D32f Feat, double angle, double hypotenuse, char OptfndFeat, CvScalar Linecolor )
{
	float alpha = 0.4f;
	int iLineThick = 1;

	CvPoint p, q;

	p.x = (int)Feat.x;
	p.y = (int)Feat.y;

	if (hypotenuse>EPSILON/*1.5*/)
	{
		q.x = (int)(p.x + 12*hypotenuse*cos(angle));

		q.y = (int)(p.y + 12*hypotenuse*sin(angle));

		cvLine(output, p, q, Linecolor, iLineThick, CV_AA, 0);

		p.x = (int)(q.x - 9*alpha*hypotenuse*cos(angle + PI/18));
		p.y = (int)(q.y - 9*alpha*hypotenuse*sin(angle + PI/18));

		cvLine(output, p, q, Linecolor, iLineThick, CV_AA, 0);

		p.x = (int)(q.x - 9*alpha*hypotenuse*cos(angle - PI/18));
		p.y = (int)(q.y - 9*alpha*hypotenuse*sin(angle - PI/18));

		cvLine(output, p, q,Linecolor, iLineThick, CV_AA, 0);
	}
}

bool OpticalFlow::mutualinfo( float *fwhist, float *bwhist, float **jointhist )
{
	if (fwhist == NULL || bwhist == NULL || jointhist == NULL)
	{
		AfxMessageBox(_T("Histogram is invalid!"));
		return false;
	}

	m_fEntropyFw = 0.0f;
	m_fEntropyBw = 0.0f;
	m_fEntropyJoint = 0.0f;

	for (int i = 0; i < HistBin; i++)
	{
		if (fwhist[i] > EPSILON)
		{
			m_fEntropyFw += -fwhist[i]*log(fwhist[i])/log(2.0f);
		}
		if (bwhist[i]> EPSILON)
		{
			m_fEntropyBw += -bwhist[i]*log(bwhist[i])/log(2.0f);
		}
		for (int j = 0; j < HistBin; j++)
		{
			if (jointhist[i][j] > EPSILON)
			{
				m_fEntropyJoint += -jointhist[i][j]*log(jointhist[i][j])/log(2.0f);
			}
		}
	}

	//Normalize, use the metric D'(x,y) = 1 - I(X,Y)/max(H(X), H(Y));

	return true;
}

void OpticalFlow::drawtext( IplImage *output, char *text, CvScalar sc, CvPoint point, CvFont *font )
{
	//sc= cvGet2D(output,20,output->height-20);
	if (sc.val[1]>128) 
		cvPutText(output, text, /*cvPoint(20,output->height-20)*/point, font, cvScalar(0,0,0));
	else
		cvPutText(output, text, /*cvPoint(20,output->height-20)*/point, font, cvScalar(255,255,255));

}

void OpticalFlow::resethist( float *fwhist, float *bwhist, float **jointhist )
{
	//FWHist = new float[HistBin];
	memset(FWHist, 0, HistBin*sizeof(float));
	//BWHist = new float[HistBin];
	memset(BWHist, 0, HistBin*sizeof(float));
	//FVJointHist = new float*[HistBin];

	for (int i = 0; i < HistBin; i++)
	{
		//FVJointHist[i] = new float[HistBin];
		memset(FVJointHist[i], 0, HistBin*sizeof(float));
	}
}

void OpticalFlow::process2( IplImage *image, int framenum )
{
	FeatureOP FeatopFw;
	FeatureOP FeatopBw;

	float fFWSum = 0.0f;
	float fBWSum = 0.0f;
	float fJointSum = 0.0f;

	float fEnergyfwtemp = 0.0f;
	float fEnergybwtemp = 0.0f;
	float fEnergyDifftemp = 0.0f;

	//int iFeatureno = m_igridheight*m_igridwidth;

	// 	CvMat* pFrameMat = NULL;
	// 	CvMat* pFrMat = NULL;
	// 	CvMat* pBkMat = NULL;

	if (framenum == 0)
	{
// 		CvSize cvImgSize = cvSize(image->width, image->height);
// 		if (m_pWriter == NULL)
// 		{
// 			m_pWriter = cvCreateVideoWriter("E:\\Desktop\\opencv_video\\abnormal.avi", CV_FOURCC('X', 'V', 'I', 'D'), 25, cvImgSize, 1);
// 		}
		cvConvertImage(image, NextImg, CV_CVTIMG_FLIP);
		cvConvertImage(image, PreImg, CV_CVTIMG_FLIP);
		cvConvertImage(image, CurImg, CV_CVTIMG_FLIP);
		cvConvertImage(image, output, CV_CVTIMG_FLIP);

		//background modeling
		pBkMat = cvCreateMat(image->height, image->width, CV_32FC1);
		pFrMat = cvCreateMat(image->height, image->width, CV_32FC1);
		pFrameMat = cvCreateMat(image->height, image->width, CV_32FC1);

		cvConvert(CurImg, pFrameMat);
		cvConvert(CurImg, pFrMat);
		cvConvert(CurImg, pBkMat);

		//gaussian model
		//bg_model = (CvGaussBGModel*)cvCreateGaussianBGModel(image, 0);
	}
	else if (framenum == 1)
	{
		cvConvertImage(image, CurImg, CV_CVTIMG_FLIP);
		cvConvertImage(image, NextImg, CV_CVTIMG_FLIP);
		cvConvertImage(image, output, CV_CVTIMG_FLIP);

		cvConvert(CurImg, pFrameMat);

		cvAbsDiff(pFrameMat, pBkMat, pFrMat);
#ifdef TEST
		cvThreshold(pFrMat, FrModel, 20, 255.0, CV_THRESH_BINARY);
#endif
		cvRunningAvg(pFrameMat, pBkMat, 0.2/*0.003*/, 0);

#ifdef TEST
		cvConvert(pBkMat, BkModel);
		//cvShowImage("foreground", FrModel);
		//cvShowImage("background", BkModel);
#endif
		//gaussian model
		//cvUpdateBGStatModel(image, (CvBGStatModel *)bg_model );
	}
	else
	{
		cvConvertImage(image, NextImg, CV_CVTIMG_FLIP);
		//cvConvertImage(image, BkImg, CV_CVTIMG_FLIP);
		cvConvertImage(image, output, CV_CVTIMG_FLIP);

		cvConvert(CurImg, pFrameMat);

		cvAbsDiff(pFrameMat, pBkMat, pFrMat);

#ifdef TEST
		cvThreshold(pFrMat, FrModel, 20, 255.0, CV_THRESH_BINARY);
#endif

		cvRunningAvg(pFrameMat, pBkMat, 0.2/*0.003*/, 0);

#ifdef TEST
		cvConvert(pBkMat, BkModel);
		//cvShowImage("foreground", FrModel);
		//cvShowImage("background", BkModel);
#endif
		//gaussian model
		//cvUpdateBGStatModel(image, (CvBGStatModel *)bg_model );

		//cvGoodFeaturesToTrack(/*PreImg*/CurImg, EigImg, TmpImg, Features, &FeatureNo, .01, 5.0, 0, 3, 0, 0.4);
		//cvFindCornerSubPix(CurImg, Features, FeatureNo, cvSize(3, 3), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3));
		//memset(PyramidPrev->imageData, 0, sizeof(float)*(image->width)*(image->height));
		//memset(PyramidCur->imageData, 0, sizeof(float)*(image->width)*(image->height));
		cvCalcOpticalFlowPyrLK(CurImg, NextImg, PyramidCur2, PyramidPrev2, m_pFeatureGrid, m_pFeatureGridtracked2, m_iFeatureno, OptWnd, 3, m_pfndFeatures2, \
			m_pOptFeatError, OptTerCri, 0);

		//memset(PyramidPrev->imageData, 0, sizeof(float)*(image->width)*(image->height));
		//memset(PyramidCur->imageData, 0, sizeof(float)*(image->width)*(image->height));
		cvCalcOpticalFlowPyrLK(PreImg, CurImg, PyramidCur, PyramidPrev, m_pFeatureGrid, m_pFeatureGridtracked, m_iFeatureno, OptWnd, 3, m_pfndFeatures, \
			m_pOptFeatError, OptTerCri, 0);



		cvCopy(CurImg, PreImg, NULL);
		cvCopy(NextImg, CurImg, NULL);

		for (int i = 0; i < m_iFeatureno; i++)
		{
			m_pOpticalflow[i].x = m_pFeatureGridtracked[i].x - m_pFeatureGrid[i].x;
			m_pOpticalflow[i].y = m_pFeatureGridtracked[i].y - m_pFeatureGrid[i].y;
			m_pOpticalflow2[i].x = m_pFeatureGridtracked2[i].x - m_pFeatureGrid[i].x;
			m_pOpticalflow2[i].y = m_pFeatureGridtracked2[i].y - m_pFeatureGrid[i].y;
		}


		//   		optmeanfilter(m_pOpticalflow, 3);
		//   		optmeanfilter(m_pOpticalflow2, 3);

		optmedianfilter(m_pOpticalflow, 3);
		optmedianfilter(m_pOpticalflow2, 3);
		optmeanfilter(m_pOpticalflow, 3);
		optmeanfilter(m_pOpticalflow2, 3);

		// 		optmedianfilter(m_pOpticalflow, 3);
		// 		optmedianfilter(m_pOpticalflow2, 3);


		//draw feature vectors
		//drawvectors(Features, Featurestrackedfw, OptfndFeaturesfw, FeatureNo, CV_RGB(0, 255, 0), &m_fEnergyfw);
		//drawvectors(Features, Featurestrackedbw, OptfndFeaturesbw, FeatureNo, CV_RGB(255, 0, 0), &m_fEnergybw);

		//Draw Vectors

		CvPoint2D32f p, q, v;
		double dangle, dhypotenuse;

		// 		fFWSum = 0.0f;
		// 		fBWSum = 0.0f;
		// 		fJointSum = 0.0;
		resethist(FWHist, BWHist, FVJointHist);

		for (int i = 0; i < m_iFeatureno; i++)
		{
			//forward
			if (i<m_igridwidth || i>(m_iFeatureno-m_igridwidth) || i%m_igridwidth == 0 || i%m_igridwidth == (m_igridwidth-1))
			{
				//continue;
			}
			else
			{
				p = m_pFeatureGrid[i];
				unsigned char temp = 1;
#ifdef TEST
				temp = CV_IMAGE_ELEM( FrModel, unsigned char, cvRound(p.y), cvRound(p.x));
#endif

				if (/*CV_IMAGE_ELEM( FrModel, unsigned char, cvRound(p.y), cvRound(p.x))*/1)
				{
					//q = Featurestrackedfw[i];
					q.x = float(int(m_pFeatureGridtracked[i].x+0.5f));
					q.y = float(int(m_pFeatureGridtracked[i].y+0.5f));

					// 				v = cvPoint2D32f(q.x-p.x, q.y-p.y);
					// 				dangle = atan2((q.y - p.y), (q.x - p.x));
					// 				dhypotenuse = sqrt((p.y - q.y)*(p.y - q.y) + (p.x - q.x)*(p.x - q.x));

					v = m_pOpticalflow[i];
					dangle = atan2(m_pOpticalflow[i].y, m_pOpticalflow[i].x);
					dhypotenuse = sqrt(m_pOpticalflow[i].x*m_pOpticalflow[i].x + m_pOpticalflow[i].y*m_pOpticalflow[i].y);

					FeatopFw.Feature = p;
					FeatopFw.Vector = v;
					FeatopFw.angle = dangle;
					FeatopFw.hypotenuse = dhypotenuse;

					if (dhypotenuse > EPSILON && dhypotenuse < 10.0)
					{
						if (CV_IMAGE_ELEM( FrModel, unsigned char, cvRound(p.y), cvRound(p.x))/*1*/)
						{
							fEnergyfwtemp+=float(dhypotenuse*dhypotenuse);
							//drawvector(p, dangle, dhypotenuse, m_pfndFeatures[i], CV_RGB(0, 255, 0));
							//FWHist[int((dangle+2*PI)/(2*PI/HistBin))%HistBin] += (float)dhypotenuse/*1.0f*/;
							FWHist[int((dangle+2*PI)/(2*PI/HistBin))%HistBin] += 1.0f;
							//fFWSum += (float)dhypotenuse/*1.0f*/;
							fFWSum += 1.0f;

						}
						//cvSeqPush(FeatVec1, &FeatopFw);
						// 				FWHist[int((dangle+2*PI)/(2*PI/HistBin))%HistBin] += (float)dhypotenuse;
						// 				FWSum += (float)dhypotenuse;
					}
					//backward
					//q = Featurestrackedbw[i];
					q.x = float(int(m_pFeatureGridtracked2[i].x+0.5f));
					q.y = float(int(m_pFeatureGridtracked2[i].y+0.5f));
					// 				v = cvPoint2D32f(q.x-p.x, q.y-p.y);
					// 				dangle = atan2(q.y-p.y, q.x-p.x);
					// 				dhypotenuse = sqrt((p.y - q.y)*(p.y - q.y) + (p.x - q.x)*(p.x - q.x));
					v = m_pOpticalflow2[i];
					dangle = atan2(m_pOpticalflow2[i].y, m_pOpticalflow2[i].x);
					dhypotenuse = sqrt(m_pOpticalflow2[i].x*m_pOpticalflow2[i].x + m_pOpticalflow2[i].y*m_pOpticalflow2[i].y);

					FeatopBw.Feature = p;
					FeatopBw.Vector = v;
					FeatopBw.angle = dangle;
					FeatopBw.hypotenuse = dhypotenuse;

					if (dhypotenuse > EPSILON && dhypotenuse < 10.0)
					{
						if (CV_IMAGE_ELEM( FrModel, unsigned char, cvRound(p.y), cvRound(p.x))/*1*/)
						{
							fEnergybwtemp +=float(dhypotenuse*dhypotenuse);
							drawvector(p, dangle, dhypotenuse, m_pfndFeatures2[i], CV_RGB(255, 0, 0));
							//BWHist[int((dangle+2*PI)/(2*PI/HistBin))%HistBin] +=dhypotenuse/*1.0f*/;
							BWHist[int((dangle+2*PI)/(2*PI/HistBin))%HistBin] += 1.0f;
							//fBWSum += dhypotenuse/*1.0f*/;
							fBWSum += 1.0f;

						}
						//cvSeqPush(FeatVec2, &FeatopBw);		
						// 				BWHist[int((dangle+2*PI)/(2*PI/HistBin))%HistBin] +=(float)dhypotenuse;
						// 				BWSum += (float)dhypotenuse;
					}

					//joint histgram
					if (FeatopFw.hypotenuse > EPSILON && FeatopBw.hypotenuse > EPSILON && FeatopFw.hypotenuse < 10.0 && FeatopBw.hypotenuse < 10.0)
					{
						if (CV_IMAGE_ELEM( FrModel, unsigned char, cvRound(p.y), cvRound(p.x))/*1*/)
						{
							//FVJointHist[int((FeatopFw.angle+2*PI)/(2*PI/HistBin))%HistBin][int((FeatopBw.angle+2*PI)/(2*PI/HistBin))%HistBin] +=(float)FeatopFw.hypotenuse*FeatopBw.hypotenuse/*1.0f*/;
							FVJointHist[int((FeatopFw.angle+2*PI)/(2*PI/HistBin))%HistBin][int((FeatopBw.angle+2*PI)/(2*PI/HistBin))%HistBin] +=1.0f;
							//fJointSum += FeatopFw.hypotenuse*FeatopBw.hypotenuse/*1.0f*/;
							fJointSum += 1.0f;
						}
					}
				}
			}
		}
		float fforground = 0.0f;
		for (int i=0; i< image->height; i++)
		{
			for (int j = 0; j<image->width; j++)
			{
				if (CV_IMAGE_ELEM( FrModel, unsigned char, i, j))//cvRound(p.y), cvRound(p.x)))
				{
					fforground += 1.0f;
				}
			}
		}
		float fbratio = fforground/(image->width*image->height);

		fEnergyDifftemp = (fEnergyfwtemp >fEnergybwtemp) ? (fEnergyfwtemp - fEnergybwtemp) : (fEnergybwtemp - fEnergyfwtemp);

		m_fEnergyfw = fEnergyfwtemp;
		m_fEnergybw = fEnergybwtemp;
		m_fEnergyDiff = fEnergyDifftemp;

		fforratio<<fbratio<<endl;
		fenergyFile<<m_fEnergyfw*fbratio<<endl;
		fenergydiffFile <<m_fEnergyDiff<<endl;
		// 		fenergyFile.flush();
		// 		fenergydiffFile.flush();


// 		CvPoint sAbnormal = cvPoint(40, image->height-40);
// 		char *pAbnormaltext = "Abnormal behavior";
// 		if (m_fEnergyfw*fbratio > 10.0)
// 		{
// 			cvPutText(image, pAbnormaltext, sAbnormal, &m_Font, cvScalar(0, 0, 255));
// 		}
		abdetection1(image, m_fEnergyfw*fbratio, 10.0f);

		//normalize histogram
		fstream file1, file2, file3;
		file1.open("E:\\Desktop\\opencv_video\\OutputFiles\\FwHist.txt", ios::out);
		file2.open("E:\\Desktop\\opencv_video\\OutputFiles\\BwHist.txt", ios::out);
		file3.open("E:\\Desktop\\opencv_video\\OutputFiles\\JointHist.txt", ios::out);

		for (int i = 0; i < HistBin; i++)
		{
			if (fFWSum > EPSILON)
			{
				//file1<<i<<" "<<FWHist[i]<<endl;
				file1<<FWHist[i]<<endl;
				FWHist[i] /= fFWSum;
			}
			if (fBWSum > EPSILON)
			{
				//file2<<i<<" "<<BWHist[i]<<endl;
				file2<<BWHist[i]<<endl;
				BWHist[i] /= fBWSum;
			}
			for (int j = 0; j < HistBin; j++)
			{			
				if (fJointSum > EPSILON)
				{
					file3<<FVJointHist[i][j]<<" ";
					FVJointHist[i][j] /= fJointSum;
				}
			}
			file3<<endl;
		}
		//file1.flush();
		file1.close();
		file2.close();
		file3.close();
		//test

		//joint histogram
		// 		for (int i = 0; i < HistBin; i++)
		// 		{
		// 			for (int j = 0; j < HistBin; j++)
		// 			{
		// 				FVJointHist[i][j] = FWHist[i]*BWHist[j];
		// 			}
		// 		}		

		float sum1=0.0f, sum2=0.0f, sum3 = 0.0f;
		for(int i = 0; i < HistBin; i++)
		{
			sum1+=FWHist[i];
			sum2+=BWHist[i];
			for (int j = 0; j < HistBin; j++)
			{
				sum3+=FVJointHist[i][j];
			}
		}

		//Mutual Information
		if (mutualinfo(FWHist, BWHist, FVJointHist))
		{
			//draw Mutual Infromation
			// 			CvScalar misc = cvGet2D(output, 20, output->height-40);
			// 			CvPoint mipoint = cvPoint(20, output->height-40);
			// 			char mitext[50];
			m_fMutualInfo = m_fEntropyFw+m_fEntropyBw-m_fEntropyJoint;
			// 			sprintf(mitext, "Mutual Info.: #%6.3f", m_fMutualInfo);

			//			drawtext(output, mitext, misc, mipoint);

			//Normalize, use the metric D'(x,y) = 1 - I(X,Y)/max(H(X), H(Y));
			float fmaxH = m_fEntropyFw >m_fEntropyBw ? m_fEntropyFw: m_fEntropyBw;
			//m_fMutualInfo = m_fEntropyFw+m_fEntropyBw-m_fEntropyJoint;
			//m_fNMutualInfo = 1 - m_fMutualInfo/fmaxH;
			if (fmaxH>EPSILON)
			{
				m_fNMutualInfo = 1 - m_fMutualInfo/fmaxH;
 			}
			else
				m_fNMutualInfo = 0.0f;
			// 
			if (m_fNMutualInfo > 1.0f - EPSILON)
			{
				m_fNMutualInfo = 1.0f;
			}
			else if (m_fNMutualInfo < EPSILON)
			{
				m_fNMutualInfo = 0.0f;
			}
			// 			CvScalar nmisc = cvGet2D(output, 20, output->height-40);
			// 			CvPoint nmipoint = cvPoint(20, output->height-60);
			//			char nmitext[50];
			//			sprintf(nmitext, "Normalized Mutual Info.: #%6.3f",m_fNMutualInfo/*(1-(m_fEntropyFw+EntropyBw-m_fEntropyJoint)/(m_fEntropyFw>=EntropyBw?m_fEntropyFw: EntropyBw)) */);

			//			drawtext(output, nmitext, nmisc, nmipoint);
			if (m_fEnergyfw*fbratio <= 0.06)
			{
				m_fMutualInfo = 0.0f;
				m_fNMutualInfo = 0.0f;
			}
			fmutualinfoFile<<m_fMutualInfo<<endl;
			fnmutualinfoFile<<m_fNMutualInfo<<endl;

			if (m_fNMutualInfo >= 0.5f)
			{
				m_iMItypeCur = 1;
			}
			else
				m_iMItypeCur = 0;
		}

		abdetection2(image, m_iMItypePrev, m_iMItypeCur);

		m_iMItypePrev = m_iMItypeCur;

		// 		if (m_fNMutualInfo > 1.0f - EPSILON)
		// 		{
		// 			m_fNMutualInfo = 1.0f;
		// 		}
		// 		else if (m_fNMutualInfo < EPSILON)
		// 		{
		// 			m_fNMutualInfo = 0.0f;
		// 		}
		fdynamicFile<<m_fEnergyDiff*m_fMutualInfo/*m_fNMutualInfo*/<<endl;
		//draw energy difference
		//		CvScalar sc = cvGet2D(output,20,output->height-20);
		//		CvPoint point = cvPoint(20, output->height-20);
		//		char text[50];
		//		sprintf(text,"Energy Difference: #%6.3f",m_fEnergyfw-m_fEnergybw);

		//		drawtext(output, text, sc, point);

		memset(m_pFeatureGrid, 0, sizeof(CvPoint2D32f)*m_iFeatureno);
		memset(m_pFeatureGridtracked, 0, sizeof(CvPoint2D32f )*m_iFeatureno);
		memset(m_pFeatureGridtracked2, 0, sizeof(CvPoint2D32f )*m_iFeatureno);

		for (int i = 0; i < m_igridheight; i++)
		{
			for (int j =0; j < m_igridwidth; j++)
			{
				m_pFeatureGrid[i*m_igridwidth + j].x = Blockwidth*(j+1);
				m_pFeatureGrid[i*m_igridwidth + j].y = Blockheight*(i+1);
				m_pFeatureGridtracked[i*m_igridwidth + j].x = Blockwidth*(j+1);
				m_pFeatureGridtracked[i*m_igridwidth + j].y = Blockheight*(i+1);
				m_pFeatureGridtracked2[i*m_igridwidth +j].x = Blockwidth*(j+1);
				m_pFeatureGridtracked2[i*m_igridwidth +j].y = Blockheight*(i+1);
			}
		}

		memset(m_pfndFeatures, 0, sizeof(char)*m_iFeatureno);
		memset(m_pfndFeatures2, 0, sizeof(char)*m_iFeatureno);

		memset(FWHist, 0, HistBin*sizeof(float));
		memset(BWHist, 0, HistBin*sizeof(float));
	}

 }

void OpticalFlow::optmeanfilter( CvPoint2D32f *pfeature, int winsize )
{
	int iRadius = (winsize -1)/2;
	float fSumX = 0.0f;
	float fSumY = 0.0f;

	for (int i = iRadius; i < m_igridheight-iRadius; i++)
	{
		for (int j = iRadius; j < m_igridwidth-iRadius; j++)
		{
			fSumX = 0.0f;
			fSumY = 0.0f;
			for (int m = -iRadius; m <= iRadius; m++)
			{
				for (int n = -iRadius; n <= iRadius; n++)
				{
					fSumX += pfeature[(i+m)*m_igridwidth+(j+n)].x;
					fSumY += pfeature[(i+m)*m_igridwidth+(j+n)].y;
				}
			}	
			pfeature[i*m_igridwidth+j].x = fSumX/(winsize*winsize);
			pfeature[i*m_igridwidth+j].y = fSumY/(winsize*winsize);
		}
	}
}

void OpticalFlow::optmedianfilter( CvPoint2D32f *pfeature, int winsize )
{
	int iRadius = (winsize-1)/2;
	float *pWindataX = new float[winsize*winsize];
	float *pWindataY = new float[winsize*winsize];
	// 	float pWindataX[9];
	// 	float pWindataY[9];
	//float *ptempX = pWindataX;
	//float *ptempY = pWindataY;
	memset(pWindataX, 0, winsize*winsize*sizeof(float));
	memset(pWindataY, 0, winsize*winsize*sizeof(float));
	//int i, j, m, n;
	for (int i = iRadius; i < m_igridheight-iRadius; i++)
	{
		for (int j = iRadius; j < m_igridwidth-iRadius; j++)
		{
			// 			ptempX = pWindataX;
			// 			ptempY = pWindataY;
			for (int m = -iRadius ; m <= iRadius; m++)
			{
				for (int n = -iRadius; n <= iRadius; n++)
				{
					// 					*ptempX = pfeature[(i+m)*m_igridwidth + (j+n)].x;
					// 					*ptempY = pfeature[(i+m)*m_igridwidth + (j+n)].y;
					// 					ptempX++;
					// 					ptempY++;
					pWindataX[(m+iRadius)*winsize+(n+iRadius)] = pfeature[(i+m)*m_igridwidth + (j+n)].x;
					pWindataY[(m+iRadius)*winsize+(n+iRadius)] = pfeature[(i+m)*m_igridwidth + (j+n)].y;

				}
			}
			pfeature[i*m_igridwidth + j].x = FindMedianValue(pWindataX, winsize*winsize);
			pfeature[i*m_igridwidth + j].y = FindMedianValue(pWindataY, winsize*winsize);
		}
	}
	delete [] pWindataX;
	delete [] pWindataY;
}

float OpticalFlow::FindMedianValue( float *parray, int iLength )
{
	//float *ptemp = parray;
	float ftemp = 0.0f;
	for (int i = 0; i < iLength; i++)
	{
		//parray = parray;
		for (int j = 0; j < iLength -1; j++)
		{

			if (parray[j]>parray[j+1]/**ptemp > *(ptemp+1)*/)
			{
				// 				ftemp = *ptemp;
				// 				*ptemp = *(ptemp+1);
				// 				*(ptemp+1) = ftemp;
				// 				ptemp++;
				ftemp = parray[j];
				parray[j] = parray[j+1];
				parray[j+1] = ftemp;
			}
			// 			else
			// 
			// 				ptemp++;
		}
	}
	return *(parray+(iLength-1)/2);
}

void OpticalFlow::abdetection1( IplImage *image, float fenergy, float fthreshold )
{
		CvPoint sAbnormal = cvPoint(40, image->height-40);
		char *pAbnormaltext = "Abnormal behavior";
		if (fenergy > fthreshold)
		{
			cvPutText(image, pAbnormaltext, sAbnormal, &m_Font, cvScalar(0, 0, 255));
		}
}

void OpticalFlow::abdetection2( IplImage *image, int miprev, int micur )
{
	if (micur>=0&&miprev>=0)
	{
		if (miprev != micur)
		{
			CvPoint sAbnormal = cvPoint(40, image->height-40);
			char *pAbnormaltext = "Abnormal behavior";
			cvPutText(image, pAbnormaltext, sAbnormal, &m_Font, cvScalar(0, 100, 255));
		}
	}
}